﻿//////////////////////////////////////////////////////////////////////////////////
//	Conversion3DX.cs
//	Managed Wiimote 3D Library
//	Written by JungHyun Kim and JungHo Kim
//	Visit http://w3dlib.codeplex.com/
//	for more information
//////////////////////////////////////////////////////////////////////////////////
using System;
using System.Linq;
using System.Text;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;

using WiimoteLib;

namespace Wiimote3DLibX
{
    /// <summary>
    /// 
    /// </summary>
    public class Conversion3D
    {
        Dictionary<Guid, int> mWiimoteMap = new Dictionary<Guid, int>();

        private WiimoteCollection _mWC;

        Point3A<double> mTempPt = new Point3A<double>();

        /// <summary>
        /// sensitivity of IR Sensing.
        /// </summary>
        public double mSensitivity;

        /// <summary>
        /// Event Handler
        /// </summary>
        public event Event3DHandler OnEvent3D;

        //The default distance between the Wiimote
        private int INTERVAL_WII;

        /// <summary>
        /// Default 
        /// </summary>
        public Conversion3D()
            : this(100)
        {
        }

        /// <summary>
        /// Default 
        /// </summary>
        public Conversion3D(int _Interval)
        {
            INTERVAL_WII = _Interval;
            mSensitivity = _Interval / 100;
            ConnectToWii();
        }

        AutoResetEvent[] mWaitHandles = new AutoResetEvent[] 
        {
            new AutoResetEvent(false),
            new AutoResetEvent(false)
        };

        IRState[] _irStates = new IRState[2];

        void WiimoteChanged(object sender, WiimoteChangedEventArgs e)
        {
            int wIndex = mWiimoteMap[((Wiimote)sender).ID];

            _irStates[wIndex] = e.WiimoteState.IRState;

            mWaitHandles[wIndex].Set();
        }

        /// <summary>
        /// Start Wiimote3D Process
        /// </summary>
        public void Run()
        {
            while (true)
            {
                // Wait for all of the terms to be Wiimote
                WaitHandle.WaitAll(mWaitHandles);

                BringIR(_irStates[0], _irStates[1]);
            }
        }

        /// <summary>
        /// Bring IR
        /// </summary>
        /// <param name="ir1">Wiimote1 IR State</param>
        /// <param name="ir2">Wiimote2 IR State</param>
        public void BringIR(IRState ir1, IRState ir2)
        {
            IRData[] irData = new IRData[] { new IRData(ir1.IRSensors), new IRData(ir2.IRSensors) };

            if (irData[0].IRCount > 0 && irData[1].IRCount > 0)
            {
                if (OnEvent3D != null)
                {
                    Point3A<double> pt = Calculate3D(irData[0][0], irData[1][0]);

                    double Displacement;

                    Displacement = Math.Sqrt(((mTempPt.PointX - pt.PointX) * (mTempPt.PointX - pt.PointX)) + ((mTempPt.PointY - pt.PointY) * (mTempPt.PointY - pt.PointY)) + ((mTempPt.PointZ - pt.PointZ) * (mTempPt.PointZ - pt.PointZ)));

                    if (mSensitivity < Displacement)
                    {
                        OnEvent3D(this, new Wiimote3DChangedEventArgs(Wiimote3DChangedEventArgs.CurrentMode.Go, pt));
                        mTempPt = pt;
                    }
                }
            }
            else
            {
                OnEvent3D(this, new Wiimote3DChangedEventArgs(Wiimote3DChangedEventArgs.CurrentMode.Stop, mTempPt));
            }
        }

        /// <summary>
        /// Connect Two Wiimote
        /// </summary>
        private void ConnectToWii()
        {
            // find all wiimotes connected to the system
            _mWC = new WiimoteCollection();
            int index = 0;

            try
            {
                _mWC.FindAllWiimotes();
            }
            catch (WiimoteNotFoundException ex)
            {
                Debug.WriteLine(ex);
            }
            catch (WiimoteException ex)
            {
                Debug.WriteLine(ex);
            }
            catch (Exception ex)
            {
                Debug.WriteLine(ex);
            }

            foreach (Wiimote wm in _mWC)
            {
                // connect it and set it up as always                
                wm.WiimoteChanged += WiimoteChanged;

                // setup the map from this wiimote's ID to that control
                mWiimoteMap[wm.ID] = index;

                wm.Connect();

                if (wm.WiimoteState.ExtensionType != ExtensionType.BalanceBoard)
                    wm.SetReportType(InputReport.IRExtensionAccel, IRSensitivity.Maximum, true);

                wm.SetLEDs(++index);
            }
        }

        /// <summary>
        /// Disconnect from the controller and stop reading data from it
        /// </summary>
        public void Disconnect()
        {
            foreach (Wiimote wm in _mWC)
                wm.Disconnect();
        }

        /// <summary>
        /// The Wrapping Function Clcuate3D
        /// </summary>
        /// <param name="pt1">Wiimote1 Point</param>
        /// <param name="pt2">Wiimote2 Point</param>
        /// <returns></returns>
        public Point3A<double> Calculate3D(Point pt1, Point pt2)
        {
            return Calculate3D(pt1.X, pt1.Y, pt2.X, pt2.Y);
        }

        /// <summary>
        /// 3D coordinates calculation
        /// </summary>
        /// <param name="x1">Wiimote1 Point x1</param>
        /// <param name="y1">Wiimote1 Point y1</param>
        /// <param name="x2">Wiimote2 Point x2</param>
        /// <param name="y2">Wiimote2 Point y2</param>
        /// <returns>Calculated 3D coordinates</returns>
        private Point3A<double> Calculate3D(int x1, int y1, int x2, int y2)
        {
            Point3A<double> vec = new Point3A<double>();
            double angle;
            double radians;
            float result;

            try
            {
                // Calculate the tangent of 22.5 degrees.
                angle = 22.5;
                radians = angle * (Math.PI / 180);
                result = (float)Math.Tan(radians);

                vec.PointZ = (INTERVAL_WII * 1300) / Math.Abs((512 - x1) - (512 - x2));

                vec.PointX = -vec.PointZ * result * (1 - ((float)x2 / 512)) - (INTERVAL_WII / 2);

                vec.PointY = -vec.PointZ * result * (1 - ((float)y1 / 512)) + (INTERVAL_WII / 2);

                //vec.X /= 100;
                //vec.Y /= 100;
                //vec.Z /= 100;
            }
            catch (System.Exception e)
            {
                Debug.WriteLine(e);
            }

            return vec;
        }
    }
}